#ifndef WALKAWARE_KINECT_CALIBRATIONDATA_H
#define WALKAWARE_KINECT_CALIBRATIONDATA_H

#include <stdio.h>

#include <WalkaWare/Shared/Vec2.h>
#include <WalkaWare/Shared/Vec3.h>
#include <WalkaWare/Shared/Mat3.h>

namespace WalkaWare
{

namespace Kinect
{

class CalibrationData
{
	public:

		typedef void            BaseType;
		typedef CalibrationData ThisType;

		CalibrationData(void)
		{
			this->clear();
		}

		~CalibrationData(void)
		{
			this->clear();
		}

		CalibrationData(const char * fileName)
		{
			(void)this->load(fileName);
		}

		void clear(void)
		{
			this->intrinsicsRGB.SetIdentity();
			for (int i=0; i<5; ++i) { this->distortionRGB[i]   = 0.0f; }
			this->sizeRGB.SetZero();
			this->rawSizeRGB.SetZero();

			this->intrinsicsDepth.SetIdentity();
			for (int i=0; i<5; ++i) { this->distortionDepth[i] = 0.0f; }
			this->sizeDepth.SetZero();
			this->rawSizeDepth.SetZero();
			this->baseOffsetDepth.SetZero();

			this->rotationMat.SetIdentity();
			this->translationVec.SetZero();
		}

		bool load(const char * fileName)
		{
			if (!this->loadCalibrationData(fileName))
			{
				this->clear();
				return false;
			}
			return true;
		}

		const Shared::Mat3f & rgbIntrinsics(void) const
		{
			return this->intrinsicsRGB;
		}

		const float * rgbDistortion(void) const
		{
			return this->distortionRGB;
		}

		const Shared::Vec2i & rgbSize(void) const
		{
			return this->sizeRGB;
		}

		const Shared::Vec2i & rgbRawSize(void) const
		{
			return this->rawSizeRGB;
		}

		const Shared::Mat3f & depthIntrinsics(void) const
		{
			return this->intrinsicsDepth;
		}

		const float * depthDistortion(void) const
		{
			return this->distortionDepth;
		}

		const Shared::Vec2i & depthSize(void) const
		{
			return this->sizeDepth;
		}

		const Shared::Vec2i & depthRawSize(void) const
		{
			return this->rawSizeDepth;
		}

		const Shared::Vec2f & depthBaseoffset(void) const
		{
			return this->baseOffsetDepth;
		}

		const Shared::Mat3f & rotation(void) const
		{
			return this->rotationMat;
		}

		const Shared::Vec3f & translation(void) const
		{
			return this->translationVec;
		}

	protected:

		Shared::Mat3f intrinsicsRGB;
		float         distortionRGB[5];
		Shared::Vec2i sizeRGB;
		Shared::Vec2i rawSizeRGB;

		Shared::Mat3f intrinsicsDepth;
		float         distortionDepth[5];
		Shared::Vec2i sizeDepth;
		Shared::Vec2i rawSizeDepth;
		Shared::Vec2f baseOffsetDepth;

		Shared::Mat3f rotationMat;
		Shared::Vec3f translationVec;

		static bool skipS(FILE * f, int n)
		{
			char waste[255];
			for (int i=0; i<n; ++i)
			{
				if (fscanf(f, "%s", waste) <= 0) return false;
			}
			return true;
		}

		static bool readInts(FILE * f, int n, int * d)
		{
			if (!skipS(f, 10)) return false;
			char waste[255];
			for (int i=0; i<n; ++i)
			{
				if (fscanf(f, "%d %s", &(d[i]), waste) <= 0) return false;
			}
			return true;
		}

		static bool readFloats(FILE * f, int n, float * d)
		{
			if (!skipS(f, 10)) return false;
			char waste[255];
			for (int i=0; i<n; ++i)
			{
				if (fscanf(f, "%f %s", &(d[i]), waste) <= 0) return false;
			}
			return true;
		}

		bool readDistortion(FILE * f, float * d)
		{
			return readFloats(f, 5, d);
		}

		bool readVec2i(FILE * f, Shared::Vec2i & v)
		{
			return readInts(f, 2, v.V());
		}

		bool readVec2f(FILE * f, Shared::Vec2f & v)
		{
			return readFloats(f, 2, v.V());
		}

		bool readVec3f(FILE * f, Shared::Vec3f & v)
		{
			return readFloats(f, 3, v.V());
		}

		static bool readMat3f(FILE * f, Shared::Mat3f & m)
		{
			if (!readFloats(f, 9, &(m[0][0]))) return false;
			//m.transposeInPlace();
			return true;
		}

		bool loadCalibrationData(const char * fileName)
		{
			if (fileName == 0) return false;

			FILE * f = fopen(fileName, "r");
			if (f == 0) return false;

			skipS(f, 1);

			if (!readMat3f      (f, this->intrinsicsRGB))   return false;
			if (!readDistortion (f, this->distortionRGB))   return false;
			if (!readMat3f      (f, this->intrinsicsDepth)) return false;
			if (!readDistortion (f, this->distortionDepth)) return false;
			if (!readMat3f      (f, this->rotationMat))     return false;
			if (!readVec3f      (f, this->translationVec))  return false;
			if (!readVec2i      (f, this->sizeRGB))         return false;
			if (!readVec2i      (f, this->rawSizeRGB))      return false;
			if (!readVec2i      (f, this->sizeDepth))       return false;
			if (!readVec2i      (f, this->rawSizeDepth))    return false;
			if (!readVec2f      (f, this->baseOffsetDepth)) return false;

			fclose(f);

			return true;
		}

};

} // end namespace Kinect

} // end namespace WalkaWare

#endif // WALKAWARE_KINECT_CALIBRATIONDATA_H
